#include "fastMarch.h"
#include "grid.h"

void FastMarch::Reinitialize() 
{
   //cout<<"\n Entrei Fast March Reinitialize\n";
   static int d123[3] = {1,dj,dk};
   //negative Phi first 
   num_iter_fm++;
   Process_Border_Face_Lists_Information();
   bool_fluid =true;
   Process_Border_Lists_Information(1);
   //for(int i = 0; i < closeSize; i++)  AddToHeap(ClosePoints[i]);
   int fluid_cells_reached = 0;
   cout<<"List_Fluid_end[0] before ="<< List_Fluid_end[0] <<"\n";
   if((list_fluid_max <= 1) || (num_iter_fm <= num_iter_min_list_fluid))
   { 
      for( index = PopHeap(); index != -1; index = PopHeap())
      {  
          fluid_cells_reached ++;
          if(gridPhi[index] > fast_march_limit_fluid)
	  {
	      do
	      {
	          gridPhi[index] *=(-1);
		  index = PopHeap();
	      }
	      while(index != -1);
	      break;
	  } 
	  if(flag[index]< AIR) continue;
          for(int jt=0;jt<=2;jt++)
          {
             gridCellUVW[jt][index]=  0.5*(gridUVW[jt][index] + gridUVW[jt][index+d123[jt]]);                     
          } 								     
          Call_FindPhi();
          FM2LS_Fluid_List->push_back(index);
       }
   }
   else
   {					 
      for( index = PopHeap(); index != -1; index = PopHeap())
      {  
         fluid_cells_reached ++;
         if(gridPhi[index] > fast_march_limit_fluid_otim) break; 
         for(int jt=0;jt<=2;jt++)
         {
             gridCellUVW[jt][index]=  0.5*(gridUVW[jt][index] + gridUVW[jt][index+d123[jt]]);                     
         } 								     
         Call_FindPhi();
         FM2LS_Fluid_List->push_back(index);
         if((flag[index] > FLUID) && (flag[index]< INFLOW))
         {  
            flag[index] = FLUID;
	    if(List_Fluid_end[0] >= (int)List_Fluid[0]->size())
	    {
	       List_Fluid[0]->push_back(index); 
	       List_Fluid_end[0]++;
	    }
	    else
	    {
	       (*List_Fluid[0])[List_Fluid_end[0]] =index;
	        List_Fluid_end[0]++;
	    }
         }   
       }
   }
   
   cout<<"List_Fluid_end[0] after ="<< List_Fluid_end[0] <<"\n";
   cout<<"fluid_cells_reached ="<< fluid_cells_reached <<"\n";
   bool_fluid =false;
   Process_Border_Lists_Information(0);
   Process_Border_Face_Lists_Information();
   //for(int i = 0; i < closeSize; i++)  AddToHeap(ClosePoints[i]);
   for( index = PopHeap(); index != -1; index = PopHeap())
   {  
       if(gridPhi[index] > fast_march_limit_air) break; 
       if(flag[index]< AIR) continue; 
       Comp_Grid_Cell_Outside_3();						     
       Call_FindPhi();
       FM2LS_Air_List->push_back(index);
   }
   int FM2LS_Fluid_size = (int)FM2LS_Fluid_List->size();
   for(int itfor= 0; itfor < FM2LS_Fluid_size; itfor ++)
   {
       int for_index = (*FM2LS_Fluid_List)[itfor];
       if(gridPhi[for_index] > 0) gridPhi[for_index] *=(-1);
   }
   if(gridPhi == gridPhi1) gridPhi = gridPhi2;
   else gridPhi = gridPhi1;
   //Done_Flag++; 
   Done_Flag -=2;
   Otherside_Flag -=2;
}
void FastMarch::Process_Border_Face_Lists_Information()
{
   int xi,yi;
   int itend = (int) List_Border_Face_Air->size();
   for(int itfor=0;itfor < itend;itfor++)
   {
       xi = (*List_Border_Face_Air)[itfor];
       yi = (*List_Border_Face_Fluid)[itfor];
       double tempvalue = gridPhi[xi] - gridPhi[yi];
       if(tempvalue > h)
       {
          gridPhi[xi] *= (h/tempvalue);
          gridPhi[yi] *= (h/tempvalue);
	  //cout<<"Alterei um valor da gridPhi na borda em"<<xi<<","<<yi<<"\n";
	  //cout<<tempvalue<<","<<gridPhi[xi]<<","<<gridPhi[yi]<<"\n";
	 
       }
   }
}
void FastMarch::Process_Border_Lists_Information(int List_index)
{
   //closeSize = 0;
   heapSize = 0;
   std::vector <int> *List_Border_local;
   List_Border_local = List_Border[List_index];
   int itend = *(Border_List_Non_Diagonal_size[List_index]);
   //cout<<"fM:List_Border_Air->size ="<< itend <<"\n";
   for(int itfor=0; itfor< itend; itfor++) 
   {
       index = (*List_Border_local)[itfor];
      // cout<<"gridCellUVW["<<index<<"]=("<<gridCellUVW[0][index]<<","<< gridCellUVW[1][index]<<","<< gridCellUVW[2][index]<<")\n";
      // cout<<"flag["<<index<<"]=("<<flag[index]<<"\n";
       gridPhi[index] *=-1; // - is to swap values for initializing negative phi
      // HeapPosition[index] = -1;
      // ClosePoints[closeSize] = index;
      // closeSize++;
      AddToHeap(index);
   }
   List_Border_local = List_Border[1 - List_index];
   itend = *(Border_List_Non_Diagonal_size[1 - List_index]);
   //cout<<"fM:List_Border_Fluid->size ="<< itend <<"\n";
   for(int itfor=0; itfor<itend; itfor++) 
   {
	index = (*List_Border_local)[itfor];
	//cout<<"gridCellUVW["<<index<<"]=("<<gridCellUVW[0][index]<<","<< gridCellUVW[1][index]<<","<< gridCellUVW[2][index]<<")\n";
        //cout<<"flag["<<index<<"]=("<<flag[index]<<"\n";
        gridPhi[index] *=-1; 
        HeapPosition[index] = Otherside_Flag;
   }
}
void FastMarch::Call_FindPhi()
{
    static int d123[3]={1,dj,dk};
    for(int it=0;it<=2;it++)
    {
        int indexM1=index+d123[it];
        int indexm1=index-d123[it];
	//cout<<"HeapPosition[indexm1] ="<<HeapPosition[indexm1]<<"\n";
        //if((HeapPosition[indexm1]<Done_Flag) && (gridCellUVW[it][index]<0))
	//if((HeapPosition[indexm1]>Done_Flag))// && (gridCellUVW[it][index]<0)) 
	if((HeapPosition[indexm1]>Done_Flag) && (flag[indexm1]>= AIR)) 
        { 
            //cout<<"Child GeneratedM1 - \n";
	    FindPhi(indexm1);  
        }  
	//cout<<"HeapPosition[indexM1] = " << HeapPosition[indexM1] << "\n"; 
        //if((HeapPosition[indexM1]<Done_Flag) && (gridCellUVW[it][index]>0))
	//if((HeapPosition[indexM1]>Done_Flag)) //&& (gridCellUVW[it][index]>0)) 
	if((HeapPosition[indexM1]>Done_Flag) && (flag[indexM1]>= AIR))   
        { 
	   // cout<<"Child Generatedm1 - \n";
            FindPhi(indexM1); 
        } 
     } 
}	                                                               
void FastMarch::FindPhi(int local_index) 
{
   static double h2=h * h;
   static int delta[3]={1,dj,dk};
   static Double phi_local[3]={0.0,0.0,0.0};
    int jt = 0;
    for(int it=0; it<=2; it++) 
    {
	if(HeapPosition[local_index+delta[it]] == Done_Flag) 
       {
           if(HeapPosition[local_index-delta[it]] == Done_Flag)
	   {
	        phi_local[jt] = fmin(gridPhi[local_index+delta[it]], gridPhi[local_index-delta[it]]); 
	   }	
	   else
	   {
		phi_local[jt] = gridPhi[local_index+delta[it]];
           }
	   if(phi_local[jt] < 0.0) return;
	   jt++;
	}
	else
	{
	    if(HeapPosition[local_index-delta[jt]]== Done_Flag)
	    {
	       phi_local[jt] = gridPhi[local_index-delta[it]];
	       if(phi_local[jt] < 0.0) return; 
	       jt++;
	    }
	} 
   }
  
  if(jt == 3)
  {

     if(phi_local[0] > phi_local[1])
    {  
       if(phi_local[0] > phi_local[2])
       {
          if((square(phi_local[0] - phi_local[1]) + square(phi_local[0] - phi_local[2])) > h2)
          {
	    jt=2;
	    phi_local[0] = phi_local[2];
	  }
       }
       else
       {
          if((square(phi_local[2] - phi_local[1]) + square(phi_local[2] - phi_local[0])) > h2)
          {
	    jt=2;
          }
       }  
    }
    else
    {
      if(phi_local[1] > phi_local[2])
      {   
         if((square(phi_local[1] - phi_local[0]) + square(phi_local[1] - phi_local[2])) > h2)
         {
	    jt=2;
	    phi_local[1] = phi_local[2];
         }  
      }
      else
      {
         if((square(phi_local[2] - phi_local[1]) + square(phi_local[2] - phi_local[0])) > h2)
         {
	    jt=2;
         }
      }  
    }
   }
   if((jt ==2) && (abs(phi_local[0] - phi_local[1]) > h))
   {
       jt=1; 
       phi_local[0] = fmin(phi_local[0],phi_local[1]);
   }
   //double b = phi_local[0];
   //double b2 = b*b;
   double b=0;
   double b2=0;
   //for(int kt=1;kt<jt; kt++)
   for(int kt=0;kt<jt; kt++)
   {
	b += phi_local[kt];
	b2 += square(phi_local[kt]);
   }
   double quotient = square(b) - Double(jt) * (b2 - square(h));
   
   // Subtituindo o texto:
   //b = phiX + phiY + phiZ;
  //quotient = square(b) - Double(a) * (square(phiX) + square(phiY) + square(phiZ) - square(h));
   if((quotient == 0.)|| (jt == 0)) {} //cout << "FindPhi: quotient = 0 ";
   else 
   {
    //cout<<"\n computing phi";
       //gridPhi[local_index] = fmin(gridPhi[local_index],(b + sqrt(quotient))/Double(jt));
      //gridPhi[local_index] = fmin(gridPhi[local_index], phi);
	//if(HeapPosition[local_index] == -1) 
	if(HeapPosition[local_index] < 0) 
        {
            //gridPhi[local_index] = (b + sqrt(quotient))/Double(jt);
	    //if(local_index ==26402)
	    //{
	    //cout<<(b + sqrt(quotient))/Double(jt)<<"\n";
	    //cout<<gridPhi[local_index]<<"\n";
	      //int a;
	      //cin>>a;
	    //}
	   if(bool_fluid) gridPhi[local_index] = fmax(gridPhi[local_index],(b + sqrt(quotient))/Double(jt));
	   // gridPhi[local_index] = (b + sqrt(quotient))/Double(jt);
	    AddToHeap(local_index);
        }
        else 
        {
	   /*if(local_index ==26402)
	    {
	      cout<<(b + sqrt(quotient))/Double(jt)<<"\n";
	      cout<<gridPhi[local_index]<<"\n";
	      int a;
	      cin>>a;
	    }*/
	   double gridPhi_temp =  (b + sqrt(quotient))/Double(jt);

          
	   if(gridPhi[local_index]> gridPhi_temp)
	   {
	      gridPhi[local_index]= gridPhi_temp; 
              UpdateHeap(local_index);
	   } 
           
         }
    }
	
}
void FastMarch::Comp_Grid_Cell_Outside_0()
{ 
   //cout<<"\n Comp_Grid_Cell_Outside \n";
    static int d123[3]={1,dj,dk};
    static double h2= h * h;
    static double epsilon=1E-15;
    static int change[25][3]={{-1,1,1},{1,-1,1},{1,1,-1},{-1,-1,1},{-1,1,-1},{1,-1,-1},{-1,-1,-1},{1,1,0},{1,0,1},{0,1,1},{-1,1,0},{1,-1,0},{-1,0,1},  
       			     {1,0,-1},{0,-1,1},{0,1,-1},{-1,-1,0},{-1,0,-1},{0,-1,-1},{1,0,0},{0,1,0},{0,0,1},{-1,0,0},{0,-1,0},{0,0,-1}};
    static int stop[8]={2,5,6,9,15,18,21,24};
    int delta_vector[3] = {0,0,0};
    double coef_sum= 0.0, coef;
    double temp_vel[3]={0.0,0.0,0.0};
		 for(int it=0;it<=2;it++)
		 {
		    int delta=0;
		    int indexM1=index+d123[it];
		    int indexm1=index-d123[it];
		    if((HeapPosition[indexM1] <= Done_Flag)  && (flag[indexM1]>= AIR))
		    {
		       if((HeapPosition[indexm1]<= Done_Flag) && (flag[indexm1]>= AIR))
		       {
		          if(gridPhi[indexm1] < gridPhi[indexM1])
			  {
		             delta = -d123[it];
			     delta_vector[it] = -1;
			  }
		          else
			  {
		             delta = d123[it] ;
			     delta_vector[it] = 1;
			  }
		       }
		       else
		       {
		          delta = d123[it];
			  delta_vector[it] = 1;
		       }
		    }
		    else
		    {
		       if((HeapPosition[indexm1]<= Done_Flag) && (flag[indexm1]>= AIR))
		       {
		          delta = -d123[it];
			  delta_vector[it] = -1;
		       }
		    }

		    if(delta!=0)
		    {
		       coef= fmax((gridPhi[index] - gridPhi[index+delta]),0.0);
		       for(int jt=0;jt<=2;jt++)
		           temp_vel[jt] += coef * gridCellUVW[jt][index+delta];
		       coef_sum += coef;
		     }
		 }
		 if(coef_sum > 0)
		 {
		   //if(temp_vel[0]*delta_vector[0]+temp_vel[1]*delta_vector[1]+temp_vel[2]*delta_vector[2]< 0)
		   if(((temp_vel[0]*delta_vector[0])<=0)&&((temp_vel[1]*delta_vector[1])<=0)&&((temp_vel[2]*delta_vector[2])<= 0))
		   {
		       for(int it=0;it<=2;it++)
		          gridCellUVW[it][index]= temp_vel[it]/coef_sum;
		   }
		   else
		   {   
                       double temp_vel_2[3]={0,0,0};;
                       double coef_sum_2;
		       double coef_sum_max = 0.0;
		       //double temp_vel_max[3];
                       double coef_cell[6]={0,0,0,0,0,0};
                       double temp_vel_cell[6][3]={{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0},{0,0,0}};
                       int coef_cell_index = 0;
                       for(int ft=0;ft<=2;ft++)
		       {
		          if(delta_vector[ft] == 0)
			     coef_cell_index +=2;
			  else
			  {
			     for(int ht=-1;ht<=1;ht+=2)
                             {
                                 if( HeapPosition[index+ht*d123[ft]] <= Done_Flag)
			         { 
			             int index_temp = index+ht*d123[ft];
                                     int coef_cell_temp = abs(gridPhi[index] - gridPhi[index_temp]);
                                     for(int it=0;it<=2;it++)
                                       temp_vel_cell[coef_cell_index][it] = coef_cell_temp * gridCellUVW[it][index_temp];
                                     coef_cell[coef_cell_index] = coef_cell_temp;
                                 }
                                 coef_cell_index ++;
                              }
			   }
   			}     
 		       //int imax = -1;
                       int lt = 0;
		       for(int it=0;it<25;it++)
                       {   
			       double vel_delta=0; 
                               int change_index[3]= {0,0,0};
			       int change_delta[3]= {0,0,0};
			       int change_delta_max[3]= {0,0,0};
                               for(int jt=0;jt<=2;jt++)
			       {      
				   change_delta[jt] = change[it][jt] * delta_vector[jt];
                                   change_index[jt] = max(change_delta[jt],0);
			       }
			       int ktx2=0;
			       for(int jt=0;jt<=2;jt++)
                                   for(int kt=0;kt<=2;kt++,ktx2 +=2)
				   {
                                      //vel_delta += change_delta[jt] *temp_vel_cell[ktx2+change_index[kt]][jt];
				      double temp_vel_delta = change_delta[jt] *temp_vel_cell[ktx2+change_index[kt]][jt];
				      vel_delta +=((temp_vel_delta>0)?1.0E8:temp_vel_delta);
				    }
			       if(vel_delta<0)
			       {
                                   coef_sum_2= 0.0;
				   int jtx2 = 0;
                                   for(int jt=0;jt<=2;jt++,jtx2 +=2)
                                       if(change_delta[jt]!= 0)coef_sum_2 += coef_cell[jtx2 + change_index[jt]];
                                   if( coef_sum_2 > coef_sum_max)
                                   {
				       coef_sum_max = coef_sum_2;
                                       for(int jt=0;jt<=2;jt++)
                                           change_delta_max[jt] = change_delta[jt];
                                    }
				              
				}
                                if(it == stop[lt])
                                {
                                    if(coef_sum_max > 0)
                                    {
					  double  phi[3]={-1E15,-1E15,-1E15};
					  int jtx2 =0;
                                          for(int jt=0;jt<=2;jt++,jtx2 +=2) 
                                          {
                                              if( change_delta_max[jt] != 0)
                                              { 
                                                 int change_index_temp = max(change_delta_max[jt],0);
                                                 for(int kt=0;kt<=2;kt++)
                                                    temp_vel_2[kt] += temp_vel_cell[jtx2+change_index_temp][kt];
					         phi[jt] = gridPhi[index+change_delta_max[jt]*d123[jt]];
						 for(int lt=0; lt<jt;lt++)
						 {
						     if(phi[lt] < phi[jt])
						     {
						        double temp = phi[lt];
							        phi[lt] = phi[jt];
								phi[jt] = temp;
						      }
						  }
                                              }
                                           }
					  int ini=0;
					  int end=2;
					  int ant_ini=0;
					  do
					  {
					     double sum2 =0;
					     for(int jt=ini+1; jt<=2; jt ++)
					     {
					        if (phi[jt]==-1E15)
						{
						    end =jt -1;
						    break;
						}
					        sum2 += square(phi[jt]-phi[ini]); 
						if(sum2 > h2) 
						{
						   ini++;
						   break;
						} 
					     }
					      
					   }
					   while(ini> ant_ini);
					   double a=Double(end - ini +1);
					   double b=0;
					   double b2 = -(h2);
					   for(int jt=ini; jt<=end; jt ++)
					   {
					       b +=phi[jt];
					       b2 += phi[jt]*phi[jt];
					    }
					    gridPhi[index] =( b + sqrt(b*b - a*b2))/a;
					    /* From the routine Find_Phi
					    b = phiX + phiY + phiZ;
	                                    quotient = square(b) - Double(a) * (square(phiX) + square(phiY) + square(phiZ) - square(h));
		                            phi = b + sqrt(quotient);
		                            phi /= Double(a);    
					   */ 
						  
                                           for(int jt=0;jt<=2;jt++)  
		                              gridCellUVW[jt][index]= temp_vel_2[jt]/coef_sum_max;
					      
                                           break;
                                    }
                                    lt++;
                                }//if(it==stop[lt])    
		         } //end_for (it<25)
                        if(coef_sum_max == 0)
			{
				for(int it=0;it<=2;it++)
		          	    gridCellUVW[it][index]= temp_vel[it]/coef_sum;
			}
		   }//end_else (temp_vel[0]*delta_vector[0]+temp_vel[1]*delta_vector[1]+temp_vel[2]*delta_vector[2]< 0) */
		 }//end_if_coef_sum 
		 else
		 {
		        /* cout<<"\n coef_sum = " << coef_sum << "em (" << index << ")\n"; 
		         for(int it=0;it<=2;it++)
		         {
		    
		            int indexM1=index+d123[it];
		            int indexm1=index-d123[it];
		            cout<<HeapPosition[indexM1]<<","<< HeapPosition[indexm1]<<","<< Done_Flag<<"\n";
			    cout<<gridPhi[indexM1]<<","<<gridPhi[indexm1]<<"\n";
			 }
			  cout<<gridPhi[index]<<"\n";
			  int a;
			  cin >> a;*/
			  for(int it=0;it<=2;it++)
		         {
		            gridCellUVW[it][index] = 0.0;
			 }
		    
		    
		 }
		 /*double norma = 0.0;
		 for(int it=0;it<=2;it++)
		         {
		           norma += fabs(gridCellUVW[it][index]);
			 }
		 if(norma == 0)
		 {
		    for(int it=0;it<=2;it++)
		         {
		    
		            int indexM1=index+d123[it];
		            int indexm1=index-d123[it];
		            cout<<HeapPosition[indexM1]<<","<< HeapPosition[indexm1]<<","<< Done_Flag<<"\n";
			    cout<<gridPhi[indexM1]<<","<<gridPhi[indexm1]<<"\n";
			 }
	            cout<<gridPhi[index]<<"\n";
	            int a;
	            cin >> a;
		 }*/
                 //cout<<"gridCellUVW[0][index] ="<< gridCellUVW[0][index] <<"\n";
                //cout<<"\n Saindo da March \n";
 }  
 void FastMarch::Comp_Grid_Cell_Outside_1()
{ 
   //cout<<"\n Comp_Grid_Cell_Outside \n";
    static int d123[3]={1,dj,dk};
    static double epsilon =1.0E-15; 
    int delta_vector[3] = {0,0,0};
    double coef_sum= 0.0, coef[3]={0.0,0.0,0.0}, coef_test[3]={0.0,0.0,0.0};
    double temp_vel[3]={0.0,0.0,0.0};
		 for(int it=0;it<=2;it++)
		 {
		    int indexM1=index+d123[it];
		    int indexm1=index-d123[it];
		    if(HeapPosition[indexM1] <= Done_Flag)

		    {
		       if(HeapPosition[indexm1]<= Done_Flag)
		       {
		          if(gridPhi[indexm1] < gridPhi[indexM1])
			  {
		             
			     delta_vector[it] = -d123[it];
			     coef[it]= fmax((gridPhi[index] - gridPhi[index-d123[it]]),0.0) + epsilon;
			     coef_test[it] =-1.0/coef[it];
			  }
		          else
			  {
		             
			     delta_vector[it] = d123[it];
			     coef[it]= fmax((gridPhi[index] - gridPhi[index+d123[it]]),0.0) + epsilon;
			     coef_test[it] =1.0/coef[it];
			  }
		       }
		       else
		       {
		          
			  delta_vector[it] = d123[it];
			  coef[it]= fmax((gridPhi[index] - gridPhi[index+d123[it]]),0.0) + epsilon;
			  coef_test[it] =1.0/coef[it];
		       }
		    }
		    else
		    {
		       if(HeapPosition[indexm1]<= Done_Flag)
		       {
		          
			  delta_vector[it] = -d123[it];
			  coef[it]= fmax((gridPhi[index] - gridPhi[index+d123[it]]),0.0) + epsilon;
			  coef_test[it] =-1.0/coef[it];
		       }
		    }

		    
		 }
		 for(int it=0;it<=2;it++)
		 {
		     if(coef[it] > 0.0)
		     {
		    	double prod_test=0.0;
		    	for(int jt=0;jt<=2;jt++)
		    	{
		       	    if(coef[jt] > 0.0)
		       	    {
		           	prod_test += gridCellUVW[jt][index+delta_vector[it]]/coef_test[jt];
		       	    }
		        }
		    	if(prod_test<0.0)
		    	{
		            for(int jt=0;jt<=2;jt++)
			    {
			        temp_vel[jt] += coef[it] * gridCellUVW[jt][index+delta_vector[it]];
			    }
			    coef_sum += coef[it];
		        }
		     }
		  }
			
		 if(coef_sum > 0)
		 {
		     for(int it=0;it<=2;it++)
		          gridCellUVW[it][index]= temp_vel[it]/coef_sum;
		 } 
		 else
		 {
			  for(int it=0;it<=2;it++)
		         {
		            gridCellUVW[it][index] = 0.0;
			 }
		    
		 }
		 
                //cout<<"\n Saindo da March \n";
 }  
 void FastMarch::Comp_Grid_Cell_Outside_2()
{ 
   //cout<<"\n Comp_Grid_Cell_Outside \n";
    static int d123[3]={1,dj,dk};
    static double h2= h * h;
    double coef_sum= 0.0, coef;
    double temp_vel[3]={0.0,0.0,0.0};
		 for(int it=0;it<=2;it++)
		 {
		    int indexM1=index+d123[it];
		    int indexm1=index-d123[it];
		    if((HeapPosition[indexM1] <= Done_Flag) && (flag[indexM1] >= AIR))
		    {
		       if(gridUVW[it][indexM1] < 0.0)
		       {
		          coef = -gridUVW[it][indexM1] * fmax((gridPhi[index] - gridPhi[indexM1]),0.0);
			  for(int jt=0;jt<=2;jt++)
		             temp_vel[jt] += coef * gridCellUVW[jt][indexM1];
			  coef_sum += coef;  
		       }
		       else if((gridUVW[it][indexM1]== 0.0) && (gridCellUVW[it][indexM1]< 0.0))
		       {
		          coef = -gridCellUVW[it][indexM1] * fmax((gridPhi[index] - gridPhi[indexM1]),0.0);
			  for(int jt=0;jt<=2;jt++)
		             temp_vel[jt] += coef * gridCellUVW[jt][indexM1];
			  coef_sum += coef;  
		       }
		    }
		    if((HeapPosition[indexm1]<= Done_Flag) && (flag[indexm1] >= AIR))
		    {
		       if(gridUVW[it][index] > 0.0)
		       {
		          coef = gridUVW[it][index] * fmax((gridPhi[index] - gridPhi[indexm1]),0.0);
			  for(int jt=0;jt<=2;jt++)
		             temp_vel[jt] += coef * gridCellUVW[jt][indexm1];
			  coef_sum += coef;  
		       }
		       else if((gridUVW[it][index]== 0.0) && (gridCellUVW[it][indexm1]> 0.0))
		       {
		          coef = gridCellUVW[it][indexm1] * fmax((gridPhi[index] - gridPhi[indexm1]),0.0);
			  for(int jt=0;jt<=2;jt++)
		             temp_vel[jt] += coef * gridCellUVW[jt][indexm1];
			  coef_sum += coef; 
		       }   
		    } 
		 } 	
		 if(coef_sum > 0)
		 {
		     for(int it=0;it<=2;it++)
		          gridCellUVW[it][index]= temp_vel[it]/coef_sum;
		 } 
		 else
		 {
			  for(int it=0;it<=2;it++)
		         {
		            gridCellUVW[it][index] = 0.0;
			 }
		    
		 }
		 
                //cout<<"\n Saindo da March \n";
 }  
  void FastMarch::Comp_Grid_Cell_Outside_3()
{ 
   //cout<<"\n Comp_Grid_Cell_Outside \n";
    static int d123[3]={1,dj,dk};
    static double h2= h * h;
    double coef_sum= 0.0, coef;
    double temp_vel[3]={0.0,0.0,0.0};
		 for(int it=0;it<=2;it++)
		 {
		    int indexM1=index+d123[it];
		    int indexm1=index-d123[it];
		    if((HeapPosition[indexM1] <= Done_Flag) && (flag[indexM1] >= AIR))
		    {
		       if(gridCellUVW[it][indexM1] < 0.0)
		       {
		          coef = -gridCellUVW[it][indexM1] * fmax((gridPhi[index] - gridPhi[indexM1]),0.0);
			  for(int jt=0;jt<=2;jt++)
		             temp_vel[jt] += coef * gridCellUVW[jt][indexM1];
			  coef_sum += coef;  
		       }
		       
		    }
		    if((HeapPosition[indexm1]<= Done_Flag) && (flag[indexm1] >= AIR))
		    {
		       if(gridCellUVW[it][indexm1] > 0.0)
		       {
		          coef = gridCellUVW[it][indexm1] * fmax((gridPhi[index] - gridPhi[indexm1]),0.0);
			  for(int jt=0;jt<=2;jt++)
		             temp_vel[jt] += coef * gridCellUVW[jt][indexm1];
			  coef_sum += coef;  
		       }
		    } 
		 } 	
		 if(coef_sum > 0)
		 {
		     for(int it=0;it<=2;it++)
		          gridCellUVW[it][index]= temp_vel[it]/coef_sum;
		 } 
		 else
		 {
			  for(int it=0;it<=2;it++)
		         {
		            gridCellUVW[it][index] = 0.0;
			 }
		    
		 }
		 
                //cout<<"\n Saindo da March \n";
 }  
void FastMarch::AddToHeap(int local_index) 
{
	FMHeap[heapSize] = local_index;
	HeapPosition[local_index] = heapSize;
	int j, i = heapSize;
	for( ; i > 0; i = j) 
        {
		j = (i-1)/2;
		if (gridPhi[ (FMHeap[i]) ] < gridPhi[ (FMHeap[j]) ] )
                {
			FMHeap[i] = FMHeap[j];
			HeapPosition[ (FMHeap[j]) ] = i;
			FMHeap[j] = local_index;
			HeapPosition[local_index] = j;
		}
		else
			break;
	}
	heapSize++; 
	//cout<<"HeapSize ="<<heapSize <<"\n";
}

void FastMarch::UpdateHeap(int local_index) 
{
	int j, i = HeapPosition[local_index];
	for( ; i > 0; i = j) 
   {
		j = (i-1)/2;
		if (gridPhi[ (FMHeap[i]) ]< gridPhi[ (FMHeap[j]) ]) 
      {
			FMHeap[i] = FMHeap[j];
			HeapPosition[ (FMHeap[j]) ] = i;
			FMHeap[j] = local_index;
			HeapPosition[local_index] = j;
		}
		else
			break;
	}
}

int FastMarch::PopHeap() 
{
	if(heapSize == 0)
		return -1;
	int j, local_index = FMHeap[0];
	HeapPosition[local_index] = Done_Flag;
	heapSize--;
	//cout<<"Popheap="<<heapSize<<"\n";
	FMHeap[0] = FMHeap[heapSize];
	HeapPosition[FMHeap[heapSize]] = 0;
	for(int i = 0; i < (heapSize-1); i = j) {
		int lc = 2*i+1;
		int rc = 2*i+2;
		Double current = gridPhi[ (FMHeap[i]) ];
		Double lv, rv;
		if(lc < heapSize) {
			lv = gridPhi[ (FMHeap[lc]) ];
			if(rc < heapSize) {
				rv = gridPhi[ (FMHeap[rc]) ];
				if(lv > rv) {
					lc = rc;
					lv = rv;
				}
			}
			if(current > lv) {
				FMHeap[i] = FMHeap[lc];
				HeapPosition[ FMHeap[i] ] = i;
				FMHeap[lc] = FMHeap[heapSize];
				HeapPosition[ FMHeap[heapSize] ] = lc;
				j = lc;
			}
			else
				break;
		}
		else
			break;
	}
	return local_index;
}
